#! /usr/bin/env python
# -*- coding: utf-8 -*-

import time

import requests


STATE_ARRIVED = 3


def set_action(action_name: str):
    url = "http://192.168.100.100:56322/rpc/aimdk.protocol.McActionService/SetAction"
    headers = {"Content-Type": "application/json"}
    payload = {
        "command": {"action": "McAction_USE_EXT_CMD", "ext_action": action_name},
    }
    rsp = requests.post(url, headers=headers, json=payload)
    rsp.raise_for_status()
    return rsp


def get_action():
    url = "http://192.168.100.100:56322/rpc/aimdk.protocol.McActionService/GetAction"
    headers = {"Content-Type": "application/json"}
    rsp = requests.post(url, headers=headers, json={})
    rsp.raise_for_status()
    return rsp.json()["info"]["current_action"]


def ensure_action(action_name: str, retries=5, retry_interval=1.0):
    try:
        for i in range(retries):
            if get_action() == action_name:
                return True
            set_action(action_name)
            time.sleep(0.1 if i == 0 else retry_interval)
    except Exception as e:
        print(f"Failed to set action: {e}")
        return False
    return False


def get_current_map_id():
    url = "http://127.0.0.1:50807/rpc/aimdk.protocol.MappingService/GetCurrentWorkingMap"
    headers = {"Content-Type": "application/json"}
    data = {"header": {}, "command": "MappingCommand_GET_CURRENT_WORKING_MAP"}
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["data"]["map_id"]


def get_navi_points(map_id: int):
    url = "http://127.0.0.1:50807/rpc/aimdk.protocol.LocalizationService/GetTopoMsgs"
    headers = {"Content-Type": "application/json"}
    data = {"header": {}, "command": "TopoCommand_GET_TOPO_MSG", "map_id": map_id}
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["data"]["points"]


# @retry(max_attempts=3, delay=1.0)
def navi_to_point(map_id: int, point_id: int):
    url = "http://127.0.0.1:39001/rpc/planning_msgs/srv/SetNavigationTask"
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
        "map_id": int(map_id),
        "type": 1,
        "target_id": int(point_id),
        "guide_line_id": 0,
        "x": 0.0,
        "y": 0.0,
        "angle": 0.0,
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    if rsp.json()["status"] is False:
        raise Exception(rsp.json()["message"])
    return True


def get_navi_status():
    url = "http://127.0.0.1:39001/rpc/planning_msgs/srv/GetNavigationState"
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["state"]


def change_robot_to_navigation_mode():
    current_action = get_action()
    print(f"Current action: {current_action}")
    # McAction_RL_LOCOMOTION_DEFAULT
    if current_action != "McAction_RL_LOCOMOTION_DEFAULT" and current_action != "McAction_RL_NAVIGATION_DEFAULT":
        print("Change action to RL_LOCOMOTION_DEFAULT")
        # exit(1)
    if not ensure_action("McAction_RL_LOCOMOTION_DEFAULT"):
        print("Change action failed")
        return False
    else:
        print("Change action succeeded")
        return True


def navigation_by_http(point_id):
    print(f"Navigation by HTTP, point id: {point_id}")

    # Gets map ID
    current_map_id = get_current_map_id()
    print(f"Current map id: {current_map_id}")

    # Gets navigation points
    navi_points = get_navi_points(current_map_id)
    print(f"Current navigation points: {navi_points}")

    # Chooses a navigation point
    for i, point in enumerate(navi_points):
        print(f"{i}: {point}")
    # choice = input("Choose a navigation point: ")
    # if choice.isdigit() and int(choice) >= 0 and int(choice) < len(navi_points):
    #     navi_point = navi_points[int(choice)]
    #     print(f"Choosed point: {navi_point}")
    # else:
    #     print("Unknown choice")
    #     exit(1)
    # point_id = navi_point["point_id"]
    print(f"Choosed point id: {point_id}")

    # Changes action
    current_action = get_action()
    print(f"Current action: {current_action}")
    # McAction_RL_LOCOMOTION_DEFAULT
    if current_action != "McAction_RL_LOCOMOTION_DEFAULT" and current_action != "McAction_RL_NAVIGATION_DEFAULT":
        print("Change action to RL_LOCOMOTION_DEFAULT")
        # exit(1)
    if not ensure_action("McAction_RL_LOCOMOTION_DEFAULT"):
        print("Change action failed")
        exit(1)

    # Executes navigation task
    navi_result = navi_to_point(current_map_id, point_id)
    if navi_result is False:
        print(
            f"Navigation failed, current map id: {current_map_id}, point id: {point_id} {navi_result}")
        exit(1)

    # Waits for navigation task to complete
    while True:
        navi_status = get_navi_status()
        print(f"Current navi status: {navi_status}")
        if navi_status == STATE_ARRIVED:
            print("Navi finished")
            break
        time.sleep(1)


def stop_navi():
    url = "http://127.0.0.1:39001/rpc/planning_msgs/srv/SetNavigationCommand"
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {"stamp": {"sec": 0, "nanosec": 0}, "frame_id": ""},
        "command": 3,
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    if rsp.json()["status"] is False:
        raise Exception(rsp.json()["message"])
    return True


if __name__ == "__main__":
    # Gets map ID
    # current_map_id = get_current_map_id()
    # print(f"Current map id: {current_map_id}")

    # # Gets navigation points
    # navi_points = get_navi_points(current_map_id)
    # print(f"Current navigation points: {navi_points}")

    # # Chooses a navigation point
    # for i, point in enumerate(navi_points):
    #     print(f"{i}: {point}")
    # choice = input("Choose a navigation point: ")
    # if choice.isdigit() and int(choice) >= 0 and int(choice) < len(navi_points):
    #     navi_point = navi_points[int(choice)]
    #     print(f"Choosed point: {navi_point}")
    # else:
    #     print("Unknown choice")
    #     exit(1)
    # point_id = navi_point["point_id"]
    # print(f"Choosed point id: {point_id}")

    # # Changes action
    # current_action = get_action()
    # print(f"Current action: {current_action}")
    # # McAction_RL_LOCOMOTION_DEFAULT
    # if current_action != "McAction_RL_LOCOMOTION_DEFAULT" and current_action != "McAction_RL_NAVIGATION_DEFAULT":
    #     print("Change action to RL_LOCOMOTION_DEFAULT")
    #     # exit(1)
    # if not ensure_action("McAction_RL_LOCOMOTION_DEFAULT"):
    #     print("Change action failed")
    #     exit(1)

    # # Executes navigation task
    # navi_result = navi_to_point(current_map_id, point_id)
    # if navi_result is False:
    #     print(
    #         f"Navigation failed, current map id: {current_map_id}, point id: {point_id} {navi_result}")
    #     exit(1)

    # # Waits for navigation task to complete
    # while True:
    #     navi_status = get_navi_status()
    #     print(f"Current navi status: {navi_status}")
    #     if navi_status == STATE_ARRIVED:
    #         print("Navi finished")
    #         break
    #     time.sleep(1)

    navigation_by_http(8)
